/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.I2C;
import java.util.Vector;

/**
 *
 * @author Administrator
 */
public class SRF08Ultra {

    private I2C m_I2c;
    private int m_distance = 0;
    private int m_light = 0;
    private int m_rangeMode = 0;
    public static final int kRangeModeInches = 0x50;
    public static final int kRangeModeCentimeters = 0x51;
    public static final int kRangeModeMicroSeconds = 0x52;
    private static Vector m_SRF08s = null;
    private static Thread m_SRF08PingThread = null;
    private static boolean m_isAutomatic = false;

    class SRF08Pinger implements Runnable {
        public void run() {
            int i = 0;
            while (m_isAutomatic) {
                SRF08Ultra SRF08i = (SRF08Ultra) m_SRF08s.elementAt(i);
                SRF08i.ping();
                i = (i + 1) % m_SRF08s.size();
                Thread.yield();
            }
        }
    }

    public SRF08Ultra(int digitalSlot, int rangeMode) {
        m_I2c = new I2C(DigitalModule.getInstance(digitalSlot), 0xE0);
        if (m_SRF08s == null)
            m_SRF08s = new Vector();
        if (m_SRF08PingThread == null) {
            m_SRF08PingThread = new Thread(new SRF08Pinger());
        }
        m_SRF08s.addElement(this);
        switch (rangeMode) {
            case kRangeModeInches:
                m_rangeMode = kRangeModeInches;
                break;
            case kRangeModeCentimeters:
                m_rangeMode = kRangeModeCentimeters;
                break;
            case kRangeModeMicroSeconds:
                m_rangeMode = kRangeModeMicroSeconds;
                break;
            default:
                DriverStationPrinter.getInstance().println("Unknown range for SRF08");
        }
    }

    public void ping() {
        if (0 == m_rangeMode) {
            DriverStationPrinter.getInstance().println("Can't Ping SRF08");
        }
        else {
            byte[] tempArray1Byte = new byte[1];
            if (m_I2c.read(0x00, 1, tempArray1Byte)) {
                DriverStationPrinter.getInstance().println("Error Reading Ultrasonic 01");
            }
            else {
                if (tempArray1Byte[0] == 10) {
                    if (m_I2c.write(0x01, 5)) {
                        DriverStationPrinter.getInstance().println("Error Writing Ultrasonic 02a");
                    }
                    else {
                        if (m_I2c.write(0x00, m_rangeMode)) {
                            DriverStationPrinter.getInstance().println("Error Writing Ultrasonic 02b");
                        }
                        else {
                            boolean isInLoop = true;
                            while (isInLoop) {
                                try {
                                    Thread.sleep(5);
                                } catch (InterruptedException e) {
                                }
                                if (!m_I2c.read(0x00, 1, tempArray1Byte)) {
                                    if (tempArray1Byte[0] == 10) {
                                        isInLoop = false;
                                    }
                                }
                            }
                            byte[] highByte = new byte[1];
                            byte[] lowByte = new byte[1];
                            m_I2c.read(2, 1, highByte);
                            m_I2c.read(3, 1, lowByte);
                            m_distance = ((((int) highByte[0]) << 8) | ((int) lowByte[0]));
                            m_I2c.read(0x01, 1, tempArray1Byte);
                            // The value is unsigned, so casting to (char) will make sure it's not negative.
                            m_light = (int)(char)tempArray1Byte[0];
                        }
                    }
                }
            }
        }
    }

    public static void setAutomaticMode(boolean bAuto) {
        if ((!m_isAutomatic) && bAuto)
            m_SRF08PingThread.start();
        m_isAutomatic = bAuto;
    }

    public static boolean isAutomatic() {
        return m_isAutomatic;
    }

    public int getRange() {
        return m_distance;
    }

    public int getLight() {
        return m_light;
    }
}
